/**
*@note Copyright 2012-2022 CETHIK All Rights Reserved.
*@brief 数据获取类文件 
*@author tuyinan
*@data 2022-04-xx
*/
#ifndef COSTMAP_2D_DATA_COLLECTOR_H_
#define COSTMAP_2D_DATA_COLLECTOR_H_

#include <config_file.h>
#include <costmap_2d/msg_struct.h>
#include <laser_geometry/laser_geometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Odometry.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <stdlib.h>
#include <tf/transform_listener.h>
#include <tf2_ros/buffer.h>

#include <Eigen/Eigen>
#include <boost/thread.hpp>
#include <iostream>
#include <mutex>

namespace costmap_2d {
/**
*@brief 数据获取类
*@author tuyinan
*@version 2022-04-xx
*/
class DataCollector {
 public:
/**
*@brief 构造函数
*@param std::string config 配置文件地址
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  DataCollector(std::string config);
/**
*@brief 析构函数
*@param 无
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  virtual ~DataCollector();
/**
*@brief 通过tf树获取机器人坐标
*@param 无
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void getOdomByTfThread();
/**
*@brief 获取机器人坐标的线程
*@param ros::NodeHandle& nh ros节点
*@param std::string odom_topic 话题名
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void createOdomSubThread(ros::NodeHandle& nh, std::string odom_topic);
/**
*@brief 获取静态地图的线程
*@param ros::NodeHandle& nh ros节点
*@param std::string odom_topic 话题名
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void createMapSubThread(ros::NodeHandle& nh, std::string map_topic);
 /**
*@brief 获取点云的线程
*@param ros::NodeHandle& nh ros节点
*@param std::string odom_topic 话题名
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void createPtsSubThread(ros::NodeHandle& nh, std::string pts_topic);

 /**
*@brief 点云回调函数
*@param const sensor_msgs::PointCloud2ConstPtr& message 点云信息
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message);
   /**
*@brief 机器人位姿回调函数
*@param const nav_msgs::Odometry::ConstPtr& odom 机器人位姿信息
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void odomCallback(const nav_msgs::Odometry::ConstPtr& odom);
     /**
*@brief 静态地图回调函数
*@param const nav_msgs::OccupancyGridConstPtr& new_map 静态地图信息
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void mapCallback(const nav_msgs::OccupancyGridConstPtr& new_map);

     /**
*@brief 获取点云
*@param costmap_2d::PointCloud& pts 点云数据
*@return bool 表示是否成功
*@author: tuyinan
*@version: 2022-04-xx
*/
  bool getPts(costmap_2d::PointCloud& pts);
       /**
*@brief 获取静态地图
*@param costmap_2d::OccupancyGrid& map 静态地图数据
*@return bool 表示是否成功
*@author: tuyinan
*@version: 2022-04-xx
*/
  bool getMap(costmap_2d::OccupancyGrid& map);
         /**
*@brief 获取机器人位姿
*@param costmap_2d::Pose& odom 机器人位姿
*@return bool 表示是否成功
*@author: tuyinan
*@version: 2022-04-xx
*/
  bool getOdom(costmap_2d::Pose& odom);

/**
*@brief 根据tf转换，从相机坐标到世界坐标
*@param costmap_2d::PointCloud& pts 点云数据
*@param tf::StampedTransform t_in tf转换
*@return void 
*@author: tuyinan
*@version: 2022-04-xx
*/
  void do_transform_camera(costmap_2d::PointCloud& pts,
                           tf::StampedTransform t_in);

 protected:
 /// 地图锁
  std::mutex map_mutex_;
  /// 位姿锁
  std::mutex odom_mutex_;
/// 点云锁
  std::mutex pts_mutex_;

/// 地图数据
  costmap_2d::OccupancyGrid map_data_;
/// 点云数据
  costmap_2d::PointCloud pt_data_;
/// 位姿数据
  costmap_2d::Pose odom_data_;

/// 地图，点云，位姿的收到时间
  ros::Time map_recevied_time_, pts_recevied_time_, odom_recevied_time_;

/// 判断是否收到了odom
  bool odom_flag_ = false;  // 用来作为收到odom的信号
/// 全局frame
  std::string global_frame_;  ///< @brief The global frame for the costmap
/// 配置文件类
  ConfigFile* configNode_;
/// 是否只接受第一张静态图
  bool first_map_only_;  ///< @brief Store the first static map and reuse it on
                         ///< reinitializing

/// 位姿接受线程
  boost::thread* odom_sub_thread_;
/// 地图接受线程
  boost::thread* map_sub_thread_;
 /// 点云接受线程
  boost::thread* pts_sub_thread_;

/// 地图，位姿，点云的监听器
  ros::Subscriber sub_map_, sub_odom_, sub_pts_;

 private:
};

}  // namespace costmap_2d

#endif  // COSTMAP_2D_OBSTACLE_DATA_COLLECTOR_H_
